//
// Created by wenxin on 7/15/20.
//
#include <iostream>
#include "ReadPlyFile.h"
#include "graph.h"
#include<unistd.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/point_cloud.h>
#include <map>
int main(){
    using namespace std;
    using namespace pcl;
    pcl::PointCloud<pcl::PointXYZ> resultCloud;//最后存储的点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud=readPly("/home/wenxin/test004.ply");
    pcl::PointCloud<pcl::PointXYZ>::Ptr kdCloud(new pcl::PointCloud<pcl::PointXYZ>());
    visualization::PCLVisualizer::Ptr viewer(new visualization::PCLVisualizer("Kd viewer"));//用于可视化kd-tree
    string rgb="rgb";
    string nameCloud="nameCloud";
    int countrgb=0;
    int countcloud=0;
    GraphAdjList graphAdjList;
    //map<pcl::PointXYZ,vector<pcl::PointXYZ> >nCloud;
 //   std::vector<std::vector<pcl::PointXYZ> > ncloud;
//    pcl::visualization::CloudViewer viewer("Cloud Viewer");//可视化点云
//    viewer.showCloud(cloud);
//    pause();
        resultCloud.points.resize(cloud->points.size ());//写入点云数据进入文件
        for (size_t i = 0; i < cloud->points.size (); ++i) {
            resultCloud.points[i].x = cloud->points[i].x;
            resultCloud.points[i].y = cloud->points[i].y;
            resultCloud.points[i].z = cloud->points[i].z;

    }
        cout<<"Point Size"<<resultCloud.points.size()<<endl;
    CreateGraphAdjList(graphAdjList,cloud->points.size(),resultCloud);
    pcl::KdTreeFLANN<pcl::PointXYZ>kdtree;
    kdtree.setInputCloud(cloud);
    pcl::PointXYZ searchPoint;
    for(int nIndex=0;nIndex < resultCloud.points.size();nIndex++){
//        searchPoint.x=resultCloud.points[0].x;
//        searchPoint.y=resultCloud.points[0].y;
//        searchPoint.z=resultCloud.points[0].z;
          searchPoint=resultCloud.points[nIndex];
        // k近邻搜索
        int K =5;
        std::vector<int>pointIdxNKNSearch(K);//记录是点云的索引
        std::vector<float>pointNKNSquaredDistance(K);
        PointCloud<pcl::PointXYZ>::Ptr copy_cloud(new PointCloud<pcl::PointXYZ>);//存储复制的索引节点
        kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance);
        //ALGraph<pcl::PointXYZ> graph(neighbor, 5, 4);
        copyPointCloud<pcl::PointXYZ>(*cloud, pointIdxNKNSearch, *copy_cloud);
        cout<<"@@@@@@@@@@@@@@@"<<searchPoint<<"    "<<pointIdxNKNSearch[0]<<"1 value  "<<pointIdxNKNSearch[1]<<endl;
        ostringstream ossRGB;
        ossRGB << rgb << countrgb;
        string nameCloud1 = ossRGB.str();
        cout << "**************" << nameCloud1 << endl;
        viewer->addPointCloud(copy_cloud,visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(copy_cloud, 255, 0, 0),nameCloud1);
        viewer->setPointCloudRenderingProperties(visualization::PCL_VISUALIZER_POINT_SIZE, 5, nameCloud1);
        for (int i = 0; i < copy_cloud->size(); i++) {
             ostringstream ossNameCloud;
             ossNameCloud << nameCloud << countcloud;
             viewer->addLine(copy_cloud->points[0], copy_cloud->points[i], ossNameCloud.str());
             countcloud++;//记录的点云数量
             cout<<"##########"<<countcloud<<endl;
        }
        countrgb++;
        CreateGraphEdge(graphAdjList,resultCloud.points.size(),4,resultCloud,pointIdxNKNSearch);
        cout<<"   "<<pointIdxNKNSearch[0]<<endl;
    }
    Unique(graphAdjList);
    Print(graphAdjList);
    viewer->spin();

  //  std::cerr << "Saved " << resultCloud.points.size () << " data points to test_pcd.ply." << std::endl;
    //pcl::io::savePLYFileASCII("/home/wenxin/resultPly",resultCloud);
//      int arry[] = { 1,2,3,4,5 };
//      ALGraph<int> graph(arry, 5, 2);
//     // graph.BFSTraverse(0);
//      cout << endl;
//      graph.DFSTraaverse(0);
      pause();
      return 0;
}
